clear all

springN = 1;


str = sprintf('../../../jhurst/current/Kemper_journal/SEDA_model/spring%d/sys%d.mat',springN,springN);
load(str);

tauc	= sys.tauc;
GR		= sys.GR;

K		= sys.K;
B		= sys.B;
I		= sys.I;
if (springN == 1)
% 	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring1/zero_trq_test.no.2950.1.log');
% 	OFFSET	= sys.OFFSET+0.2;
	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring1/zero_trq.1.2950.log');
	OFFSET	= sys.OFFSET-0.005;
	f = 2950/512;
elseif (springN == 2)
% 	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring2/zero_trq_test.no.2300.2.log');
% 	OFFSET	= sys.OFFSET+0.2;
	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring2/zero_trq.2.2300.log');
	OFFSET	= sys.OFFSET+0.208;
	f = 2300/512;
else
	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring3/zero_trq_test.no.1600.3.log');
	OFFSET	= sys.OFFSET+0.21;
% 	log = importdata('../../../jhurst/current/Kemper_journal/SEDA_model/spring3/zero_trq.3.1600.log');
% 	OFFSET	= sys.OFFSET+0.2085;
	f = 1600/512;
end

A2T = tauc*GR;

a	= 250.0380e-003/2;
w	= 2*pi*f;
% w	= 2*pi*(rpm/60);
% w	= 3.185;
b	= 3.05;

[tmp ti] = max(log.data(:,5));
t = log.data(ti:end,1)-log.data(ti,1);
tend = find(t >= 2/f,1);
t = t(1:tend);

motor_angle		= log.data(ti:ti+tend-1,4);
leg_angle		= log.data(ti:ti+tend-1,5);

motor_vel		= log.data(ti:ti+tend-1,6);
leg_vel			= log.data(ti:ti+tend-1,7);

motor_current	= log.data(ti:ti+tend-1,8);


mtr_trq = motor_current*A2T;

spring_pos = motor_angle-leg_angle + OFFSET;
spring_vel = motor_vel - leg_vel;

spring_trq = K*spring_pos + B*spring_vel;


W = ceil(length(t)*0.03);
if (W>10)
	W = 10;
end

temporary	= filter( repmat( 1/2/W, 1, W ), 1, spring_trq( end : -1 : 1, : ) );
filt_s		= filter( repmat( 1/2/W, 1, W ), 1, spring_trq ) ...
			+  temporary( end : -1 : 1, : );

spring_trq = filt_s;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\


figure
hold on
pl = plot(t,leg_angle,'b','linewidth',2);
pm = plot(t,motor_angle+OFFSET,'g','linewidth',2);

plot(t,b+a*cos(w*t),'r--','linewidth',2);
title('Angles');
axis tight
legend([pm;pl],'motor','leg');


%%%%
motor_vel2 = motor_vel;
alpha = 0.2;
for i=2:length(motor_vel)
	motor_vel2(i) = motor_vel(i)*alpha + motor_vel2(i-1)*(1-alpha);
end
figure
hold on
plot(motor_vel,'b','linewidth',2);
plot(motor_vel2,'r','linewidth',2);

%%%%%%%%
figure
hold on
plot([t(1) t(end)],[0 0],'k','linewidth',1);
plot([t(1) t(end)],[5 5],'k','linewidth',1);
plot([t(1) t(end)],[-5 -5],'k','linewidth',1);
pm = plot(t,mtr_trq,'g','linewidth',2);
ps = plot(t,spring_trq,'linewidth',2);
title('Torque')
legend([ps; pm],'Spring Torque', 'Motor Torque')

% figure
% hold on
% plot(t,off,'k','linewidth',1);
% p1 = plot(t,log.data(:,8)*tc*gr,'linewidth',2);
% title('Torque')
% legend(p1,['Motor Torque'])

